# include "geometry3d.hpp"

# include <cmath>
# include <cstring>

# include "linalg.hpp"

namespace {

  namespace g3 = geometry::d3;

  using linalg::Matrix;

  /*! @brief Get vector from the matrix
   */
  inline g3::Vector getVector(linalg::Matrix const& m, size_t column) {
    return g3::Vector(m(0, column), m(1, column), m(2, column));
  }

  /*! @brief Computes center of the point set
   */
  inline g3::Point computeCenterPoint(std::vector<g3::Point> const& v) {
    g3::Point center = {0, 0, 0};
    for (g3::Point const& p: v) {
      center.x() += p.x();
      center.y() += p.y();
      center.z() += p.z();
    }

    center.x() /= v.size();
    center.y() /= v.size();
    center.z() /= v.size();

    return center;
  }

  /*! @brief Kernel for spline
   */
  struct ThinPlateKernel {

    inline double operator()(double const r) const {
      return r < 1.e-9 ? 0 : r*r*log(r);
    }

  };

  /*! @brief Compute kernel matrix of transformation
   * 
   * @param kernel kernel function of the spline
   * @param points point location after transformation
   * @param m number of points
   */
  template <typename K>
  Matrix computeKernelMatrix(K const& kernel, std::vector<g3::Point> const& points) {
    size_t const m = points.size() / 3;

    Matrix G(m + 4, m + 4);

    for (size_t i = 0; i < m; ++i)
      for (size_t j = i; j < m; ++j)
        G(i, j) = G(j, i) = 
            kernel(distance(points[i], points[j]));

    for (size_t i = 0; i < m; ++i) {
      G(i, m) = G(m, i) = 1;
      g3::Point const& p = points[i];
      G(i, m + 1) = G(m + 1, i) = p.x();
      G(i, m + 2) = G(m + 2, i) = p.y();
      G(i, m + 3) = G(m + 3, i) = p.z();
    }

    G.LUD();

    return std::move(G);
  }

}


/*! @brief Computes fitting plane
 */
g3::CoordinateSystem g3::computeFittingPlane(std::vector<g3::Point> const& cloud) {
  using linalg::Matrix;
  using linalg::svd;

  Point const center = computeCenterPoint(cloud);

  Matrix C(3, 3);
  for (Point const& p : cloud) {
    double const dx = p.x() - center.x();
    double const dy = p.y() - center.y();
    double const dz = p.z() - center.z();

    C(0, 0) += dx * dx;
    C(0, 1) += dx * dy;
    C(0, 2) += dx * dz;

    C(1, 1) += dy * dy;
    C(1, 2) += dy * dz;

    C(2, 2) += dz * dz;
  }
  
  C(1, 0) = C(0, 1);
  C(2, 0) = C(0, 2);
  C(2, 1) = C(1, 2);

  Matrix u, s, vt;
  svd(C, u, s, vt);

  return g3::CoordinateSystem(center, 
    getVector(u, 0), getVector(u, 1), getVector(u, 2));
}


/*! @brief Morph object
 *
 * @param[in] target target markers location
 * @param[in] model model markers location
 * @param[in] mesh points of the mesh
 * @return morphed mesh
 */
std::vector<g3::Point> g3::morph(std::vector<g3::Point> const& target, std::vector<g3::Point> const& model, std::vector<g3::Point> const& mesh) {

  typedef linalg::Matrix Matrix;
  typedef linalg::Array Array;

  // Compute kernel matrix.
  Matrix G = computeKernelMatrix(ThinPlateKernel(), model);

  Matrix rhs(3, model.size() + 4);
  Array rx = rhs.row(0), ry = rhs.row(1), rz = rhs.row(2);
  for (size_t i = 0; i < model.size(); ++i) {
      Point const& np = target[i];
      Point const& op = model[i];
      rx(i) = np.x() - op.x();
      ry(i) = np.y() - op.y();
      rz(i) = np.z() - op.z();
  }

  G.LUD();
  solve(G, rhs);

  // Compute the result
  std::vector<g3::Point> rv;
  rv.reserve(mesh.size());

  size_t const nm = model.size();

  for (g3::Point const& p : mesh) {
    double dx = 0, dy = 0, dz = 0;
    for (size_t j = 0; j < model.size(); ++j) {
      Point const& q = model[j];
      double const w = ThinPlateKernel()(distance(p, q));
      dx += w * rx(j);
      dy += w * ry(j);
      dz += w * rz(j);
    }

    dx += rx(nm) + (rx(nm + 1) + 1) * p.x() 
                 +  rx(nm + 2) * p.y()
                 +  rx(nm + 3) * p.z();

    dy += ry(nm) +  ry(nm + 1) * p.x()
                 + (ry(nm + 2) + 1) * p.y()
                 +  ry(nm + 3) *p.z();

    dz += rz(nm) +  rz(nm + 1) * p.x()
                 +  rz(nm + 2) * p.y() 
                 + (rz(nm + 3) + 1) * p.z();

    rv.push_back(g3::Point(dx, dy, dz));
  }

  return std::move(rv);
}


/*! @brief Computes intersection of line and the plane
 *
 * @param[in] A first point on the line
 * @param[in] B second point on the line
 * @param[out] P point of intersection
 */
//void geometry::Plane::computeIntersection(Point const A, Point const B, Point P) const throw() {
//  double const ex = B[0]-A[0];
//  double const ey = B[1]-A[1];
//  double const ez = B[2]-A[2];

//  double const dx = p_[0]-A[0];
//  double const dy = p_[1]-A[1];
//  double const dz = p_[2]-A[2];

//  double const t = (dx*n_.x()+dy*n_.y()+dz*n_.z())/
//    (ex*n_.x()+ey*n_.y()+ez*n_.z());

//  P[0] = A[0]+t*ex;
//  P[1] = A[1]+t*ey;
//  P[2] = A[2]+t*ez;
//}


/*! @brief Computes intersection of mesh and the plane
 *
 * @param[in] p plane of intersection
 * @return set of points
 */
//std::vector<double> geometry::Mesh::computesIntersection(geometry::Plane const& p) const throw() {
//  std::vector<double> rv;

//  PointContainer pc = PointContainer(points_, 3);
//  for (size_t i = 0; i < edges_.size(); i += 2) {
//    Point const x1 = pc[edges_[i]];
//    Point const x2 = pc[edges_[i+1]];

//    if (p(x1)*p(x2) < 0) {
//      double ip[] = {0, 0, 0};
//      p.computeIntersection(x1, x2, ip);
//      rv.push_back(ip[0]);
//      rv.push_back(ip[1]);
//      rv.push_back(ip[2]);
//    } 
//  }

//  return std::move(rv);

//}

